#include"fast_lio/pc_handle.h"
#include"fast_lio/frame.h"
#include"fast_lio/frontend.h"
#include"fast_lio/show_pub.h"

fast_lio::PCHandle::Ptr p_pc_handle_;
fast_lio::Frontend::Ptr p_frontend_;
fast_lio::ShowPub::Ptr p_show_pub_;
fast_lio::Config::Ptr p_config_;

deque<sensor_msgs::Imu::ConstPtr> imu_deq_;
mutex imu_deq_mutex_;
deque<pair<double,pcl::PointCloud<PointType>::Ptr>> pc_deq_;
mutex pc_deq_mutex_;
double last_timestamp_lidar_ = -1;
double last_timestamp_imu_ = -1;
double lidar_scan_time_ = 0.1;

void getPCCB(const livox_ros_driver::CustomMsg::ConstPtr &msg){
    // 检查lidar时间戳，不能小于-1，也不能小于后一帧点云的时间
    if (msg->header.stamp.toSec() < last_timestamp_lidar_)
    {
        ROS_ERROR("lidar loop back, clear buffer");
    }
    last_timestamp_lidar_ = msg->header.stamp.toSec();
    pcl::PointCloud<PointType>::Ptr ptr;
    p_pc_handle_->preHandle(msg, ptr); // 预处理，包含数据读取
    {
        lock_guard<mutex> lck(pc_deq_mutex_);
        pc_deq_.push_back(make_pair(msg->header.stamp.toSec(),ptr));
    }
    {
        lock_guard<mutex> lck(pc_deq_mutex_);
        pcl::PointCloud<PointType>::Ptr p_pc;
        double pc_start_time = pc_deq_.front().first;
        p_pc = pc_deq_.front().second;
        double pc_last_point_time=0;
        if(p_pc->points.size()<=1){
            pc_last_point_time = pc_start_time + lidar_scan_time_;
            ROS_WARN("not enough input point cloud!");
        }else if(p_pc->points.back().curvature<0.5*lidar_scan_time_){
            pc_last_point_time = pc_start_time + lidar_scan_time_;
        }else{
            pc_last_point_time = pc_start_time + p_pc->points.back().curvature;
            lidar_scan_time_ = p_pc->points.back().curvature;
        }
        lock_guard<mutex> imu_lck(imu_deq_mutex_);
        if(imu_deq_.empty()||
            imu_deq_.back()->header.stamp.toSec()<pc_last_point_time)
            return;
        
        pcl::transformPointCloud(*ptr,*ptr,p_config_->ext_T_imu_lidar_);
        fast_lio::Frame::Ptr p_frame(new fast_lio::Frame(p_pc->points.front().curvature,ptr));
        while ((!imu_deq_.empty()) && 
            (imu_deq_.front()->header.stamp.toSec() <= pc_last_point_time)) // imu的时间必须比lidar的时间大或相等
        {
            p_frame->imu_deq_.push_back(imu_deq_.front());
            imu_deq_.pop_front();
        }
        p_frontend_->addFrame(p_frame);
        pc_deq_.pop_front();
    }
}

void getIMUCB(const sensor_msgs::Imu::ConstPtr& imu_raw){
    // 检查IMU时间戳，当前帧时间大于上一帧时间
    if (imu_raw->header.stamp.toSec() < last_timestamp_imu_)
    {
        ROS_WARN("imu loop back, clear buffer");
    }
    last_timestamp_imu_ = imu_raw->header.stamp.toSec();
    {
        lock_guard<mutex> lck(imu_deq_mutex_);
        imu_deq_.push_back(imu_raw);
    }
}

void frontendThread(){
    while(true){
        if(p_frontend_->frontendHandle()){
            // p_show_pub_->pubFull(p_frontend_->p_map_->getLocalMap());
            // p_show_pub_->pubMap(p_frontend_->p_last_frame_->pc_surface_);
            // p_show_pub_->pubMap(p_feature_extract_->pc_surface_);
            pcl::PointCloud<PointType>::Ptr p_pc(new pcl::PointCloud<PointType>);
            pcl::transformPointCloud(*p_frontend_->p_current_frame_->pc_full_, *p_pc, 
                        fast_lio::SE3toEigenM(p_frontend_->p_current_frame_->getPose_world_imu()));
            p_show_pub_->pubFull(p_pc);
            double var[7];
            fast_lio::SE3toRt(p_frontend_->p_current_frame_->getPose_world_imu(),var);
            p_show_pub_->pubOdom(var);
        }
        ros::Duration(0.000001).sleep();
    }
}

int main(int argc, char** argv)
{
    if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,ros::console::levels::Debug))
    {
        ros::console::notifyLoggerLevelsChanged();
    }

    ros::init(argc, argv, "fast_lio");
    ros::NodeHandle nh;

    p_config_.reset(new fast_lio::Config(nh));
    p_pc_handle_.reset(new fast_lio::PCHandle(p_config_));

    p_frontend_.reset(new fast_lio::Frontend(p_config_));

    p_show_pub_.reset(new fast_lio::ShowPub(nh));

    ros::Subscriber pc_sub_ = nh.subscribe(p_config_->pointcloud_topic_, 20000, getPCCB, ros::TransportHints().tcpNoDelay());
    ros::Subscriber imu_sub_ = nh.subscribe<sensor_msgs::Imu>(p_config_->imu_topic_, 20000, getIMUCB, ros::TransportHints().tcpNoDelay());
    
    thread frontend_thread(frontendThread);
    frontend_thread.detach();

    // thread imu_integ_thread(imuIntegrationThread);
    // imu_integ_thread.detach();

    // thread imu_preinteg_thread(imuPreintegrationThread);
    // imu_preinteg_thread.detach();

    // thread loop_detect_thread(loopDetectThread);
    // loop_detect_thread.detach();

    ros::MultiThreadedSpinner spinner(2);
    spinner.spin();
    // ros::spin();

    return 0;
}
